import smbus
import time
import math


class PCA9685:
    def __init__(self, address=0x40, debug=False):
        self.address = address
        self.debug = debug
        self.bus = smbus.SMBus(1)
        self.bus.write_byte_data(self.address, 0x00, 0x00)
        self.setPWMFreq(50)

    def setPWMFreq(self, freq):
        prescaleval = 25000000.0
        prescaleval /= 4096.0
        prescaleval /= float(freq)
        prescaleval -= 1.0
        prescale = int(math.floor(prescaleval + 0.5))

        oldmode = self.bus.read_byte_data(self.address, 0x00)
        newmode = (oldmode & 0x7F) | 0x10
        self.bus.write_byte_data(self.address, 0x00, newmode)
        self.bus.write_byte_data(self.address, 0xFE, prescale)
        self.bus.write_byte_data(self.address, 0x00, oldmode)
        time.sleep(0.005)
        self.bus.write_byte_data(self.address, 0x00, oldmode | 0xa1)

    def setPWM(self, channel, on, off):
        self.bus.write_byte_data(self.address, 0x06 + 4 * channel, on & 0xFF)
        self.bus.write_byte_data(self.address, 0x07 + 4 * channel, (on >> 8) & 0xFF)
        self.bus.write_byte_data(self.address, 0x08 + 4 * channel, off & 0xFF)
        self.bus.write_byte_data(self.address, 0x09 + 4 * channel, (off >> 8) & 0xFF)


# 初始化PCA9685
pwm = PCA9685(0x40, debug=False)

# 定义舵机通道
HORIZONTAL_SERVO_CHANNEL = 9  # 水平舵机
VERTICAL_SERVO_CHANNEL = 10  # 垂直舵机

# 舵机参数
HORIZONTAL_CENTER = 90  # 水平中心位置(度)
VERTICAL_CENTER = 90  # 垂直中心位置(度)
MAX_ANGLE = 45  # 最大转动角度(相对于中心)


def map_servo_angle(error, max_error, center_angle, max_angle):
    """将误差映射到舵机角度"""
    # 限制误差范围
    error = max(min(error, max_error), -max_error)

    # 计算角度变化 (线性映射)
    angle_change = (error / max_error) * max_angle

    # 计算最终角度
    target_angle = center_angle + angle_change

    # 限制角度范围
    return max(min(target_angle, center_angle + max_angle), center_angle - max_angle)


def set_servo_angle(channel, angle):
    """设置舵机角度(0-180度)"""
    # 舵机角度范围0-180度，对应占空比2.5-12.5ms
    # PCA9685的占空比范围是0-4095
    pulse = int((angle / 18 + 2.5) / 20 * 4095)  # 20ms周期(50Hz)
    pwm.setPWM(channel, 0, pulse)


def adjust_servos(error_x, error_y):
    """根据误差调整舵机位置"""
    # 映射水平误差到舵机角度
    horizontal_angle = map_servo_angle(error_x, 100, HORIZONTAL_CENTER, MAX_ANGLE)

    # 映射垂直误差到舵机角度 (注意方向可能需要根据实际机械结构调整)
    vertical_angle = map_servo_angle(-error_y, 100, VERTICAL_CENTER, MAX_ANGLE)

    # 设置舵机角度
    set_servo_angle(HORIZONTAL_SERVO_CHANNEL, horizontal_angle)
    set_servo_angle(VERTICAL_SERVO_CHANNEL, vertical_angle)

    print(f"调整舵机 - 水平: {horizontal_angle:.1f}°, 垂直: {vertical_angle:.1f}°")


def parse_openmv_data(data):
    """解析OpenMV发送的数据"""
    try:
        # 数据格式: $X,Y#\n
        if data.startswith('$') and '#' in data:
            parts = data[1:-2].split(',')
            if len(parts) == 2:
                error_x = int(parts[0])
                error_y = int(parts[1])
                return error_x, error_y
    except Exception as e:
        print(f"解析数据错误: {e}")
    return 0, 0


def main():
    # 初始化舵机到中心位置
    set_servo_angle(HORIZONTAL_SERVO_CHANNEL, HORIZONTAL_CENTER)
    set_servo_angle(VERTICAL_SERVO_CHANNEL, VERTICAL_CENTER)
    time.sleep(1)  # 等待舵机就位

    # 初始化UART (与OpenMV相同的设置)
    # 注意: 树莓派通常使用GPIO14(TX)和GPIO15(RX)，对应UART0
    # 需要根据实际连接调整

    print("等待OpenMV数据...")

    try:
        import serial
        ser = serial.Serial('/dev/ttyS0', 9600, timeout=1)  # 根据实际端口调整

        while True:
            if ser.in_waiting > 0:
                line = ser.readline().decode('utf-8').strip()
                if line.startswith('$') and line.endswith('#'):
                    error_x, error_y = parse_openmv_data(line)
                    adjust_servos(error_x, error_y)

    except KeyboardInterrupt:
        print("程序被用户中断")
    except Exception as e:
        print(f"发生错误: {e}")
    finally:
        # 返回舵机到中心位置
        set_servo_angle(HORIZONTAL_SERVO_CHANNEL, HORIZONTAL_CENTER)
        set_servo_angle(VERTICAL_SERVO_CHANNEL, VERTICAL_CENTER)
        if 'ser' in locals():
            ser.close()


if __name__ == "__main__":
    main()